#include "geometry_msgs/msg/pose.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_msgs/action/robot_goto.hpp"
#include "robot_msgs/msg/robot_status.hpp"

class ServerNode : public rclcpp::Node
{
  private:
	// action server
	rclcpp_action::Server<robot_msgs::action::RobotGoto>::SharedPtr action;
	robot_msgs::msg::RobotStatus pose;

	// thread function
	void moveto(
		const std::shared_ptr<rclcpp_action::ServerGoalHandle<robot_msgs::action::RobotGoto>>
			goalhandle)
	{
		int far = 1;
		int tim = 0;
		auto goal = goalhandle->get_goal();
		auto result = std::make_shared<robot_msgs::action::RobotGoto::Result>();
		// move to target position, step by step
		RCLCPP_INFO(this->get_logger(), "moving to target position...");
		rclcpp::Rate rate = rclcpp::Rate(10);
		while (rclcpp::ok() && far)
		{
			far = 0;
			if (abs(pose.position.position.x - goal->goal.position.x) > 0.5)
			{
				pose.position.position.x +=
					goal->goal.position.x > pose.position.position.x ? 1 : -1;
				far++;
			}

			if (abs(pose.position.position.y - goal->goal.position.y) > 0.5)
			{
				pose.position.position.y +=
					goal->goal.position.y > pose.position.position.y ? 1 : -1;
				far++;
			}
			auto feedback = std::make_shared<robot_msgs::action::RobotGoto::Feedback>();
			feedback->pose.set__position(pose.position.position);
			feedback->cycle = tim;
			tim++;
			goalhandle->publish_feedback(feedback);
			if (goalhandle->is_canceling())
			{

				result->pos.position.x = pose.position.position.x;
				result->pos.position.y = pose.position.position.y;
				goalhandle->canceled(result);
				RCLCPP_INFO(this->get_logger(), "goal cancelled by client");
				return;
			}
			RCLCPP_INFO(this->get_logger(), "moving...(x:%f, y:%f, t:%d)", pose.position.position.x, pose.position.position.y, tim);
			rate.sleep();
		}
		result->pos.set__position(pose.position.position);
		goalhandle->succeed(result);
		RCLCPP_INFO(this->get_logger(), "reached target position(x:%f, y:%f, t:%d)", pose.position.position.x, pose.position.position.y, tim);
	}

	// called every time a goal is received, return wither its accepted or not
	rclcpp_action::GoalResponse goal_callback(
		const rclcpp_action::GoalUUID &,
		std::shared_ptr<const robot_msgs::action::RobotGoto::Goal> goal)
	{
		RCLCPP_INFO(this->get_logger(), "goal received");
		if (goal->goal.position.z == 0)
		{
			RCLCPP_INFO(this->get_logger(), "target position accepted(x:%f, y:%f)",
						goal->goal.position.x, goal->goal.position.y);
			return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
		}
		else
		{
			RCLCPP_INFO(this->get_logger(), "target position z not equal zero, reject");
			return rclcpp_action::GoalResponse::REJECT;
		}
	}

	// called every time a goal is cancelled by client
	rclcpp_action::CancelResponse cancel_callback(
		const std::shared_ptr<rclcpp_action::ServerGoalHandle<robot_msgs::action::RobotGoto>>)
	{
		RCLCPP_INFO(this->get_logger(), "target position cancelled by client and its ok");
		return rclcpp_action::CancelResponse::ACCEPT;
	}

	// called every time an action request is accepted by server and is going to be carried out
	void accepted_callback(
		const std::shared_ptr<rclcpp_action::ServerGoalHandle<robot_msgs::action::RobotGoto>>
			goalhandle)
	{
		// actually carry out the action
		std::thread(std::bind(&ServerNode::moveto, this, std::placeholders::_1), goalhandle)
			.detach();
	}

  public:
	ServerNode(std::string name) : Node(name)
	{
		RCLCPP_INFO(this->get_logger(), "server node started");
		this->action = rclcpp_action::create_server<robot_msgs::action::RobotGoto>(
			this, "RobotGoto",
			std::bind(&ServerNode::goal_callback, this, std::placeholders::_1,
					  std::placeholders::_2),
			std::bind(&ServerNode::cancel_callback, this, std::placeholders::_1),
			std::bind(&ServerNode::accepted_callback, this, std::placeholders::_1));
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<ServerNode>("server_node");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
